#include <stdio.h>
#include <math.h>
#include "ParkingBackward.h"

/***********************************************/
/*   Default methods for membership functions  */
/***********************************************/

static double _defaultMFcompute_smeq(MembershipFunction mf, double x) {
 double y,mu,degree=0;
 for(y=mf.max; y>=x ; y-=mf.step)
  if((mu = mf.compute_eq(mf,y))>degree) degree=mu;
 return degree;
}

static double _defaultMFcompute_greq(MembershipFunction mf, double x) {
 double y,mu,degree=0;
 for(y=mf.min; y<=x ; y+=mf.step)
   if((mu = mf.compute_eq(mf,y))>degree) degree=mu;
 return degree;
}

static double _defaultMFcenter(MembershipFunction mf) {
 return 0;
}

static double _defaultMFbasis(MembershipFunction mf) {
 return 0;
}

/***********************************************/
/* Common functions for defuzzification methods*/
/***********************************************/

static double compute(FuzzyNumber fn, double x) {
 double dom,imp;
 int i;
 if(fn.length == 0) return 0;
 imp = fn.op.imp(fn.degree[0],fn.conc[0].compute_eq(fn.conc[0],x));
 dom = imp;
 for(i=1; i<fn.length; i++) {
  imp = fn.op.imp(fn.degree[i],fn.conc[i].compute_eq(fn.conc[i],x));
  dom = fn.op.also(dom,imp);
 }
 return dom;
}

static double center(MembershipFunction mf) {
 return mf.center(mf);
}

static double basis(MembershipFunction mf) {
 return mf.basis(mf);
}

static double param(MembershipFunction mf,int i) {
 return mf.param[i];
}

/***********************************************/
/*  Common functions to create fuzzy numbers   */
/***********************************************/

static double MF_xfl_singleton_equal(MembershipFunction _mf,double x);

static FuzzyNumber createCrispNumber(double value) {
 FuzzyNumber fn;
 fn.crisp = 1;
 fn.crispvalue = value;
 fn.length = 0;
 fn.degree = NULL;
 fn.conc = NULL;
 fn.inputlength = 0;
 fn.input = NULL;
 return fn;
}

static FuzzyNumber createFuzzyNumber(int length, int inputlength,
                                     double*input, OperatorSet op) {
 int i;
 FuzzyNumber fn;
 fn.crisp = 0;
 fn.crispvalue = 0;
 fn.length = length;
 fn.degree = (double *) malloc(length*sizeof(double));
 fn.conc = (MembershipFunction *) malloc(length*sizeof(MembershipFunction));
 for(i=0; i<length; i++) fn.degree[i] = 0;
 fn.inputlength = inputlength;
 fn.input = input;
 fn.op = op;
 return fn;
}

static int isDiscreteFuzzyNumber(FuzzyNumber fn) {
 int i;
 if(fn.crisp) return 0;
 for(i=0; i<fn.length; i++)
  if(fn.conc[i].compute_eq != MF_xfl_singleton_equal)
   return 0;
 return 1;
}

/***********************************************/
/*  Functions to compute single propositions   */
/***********************************************/

static double _isEqual(MembershipFunction mf, FuzzyNumber fn) {
 int i;
 double x,mu1,mu2,minmu,degree=0;
 if(fn.crisp) return mf.compute_eq(mf,fn.crispvalue);
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = mf.compute_eq(mf,fn.conc[i].param[0]);
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = mf.compute_eq(mf,x);
   mu2 = compute(fn,x);
   minmu = (mu1<mu2 ? mu1 : mu2);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isGreaterOrEqual(MembershipFunction mf, FuzzyNumber fn) {
 int i;
 double x,mu1,mu2,minmu,degree=0,greq=0;
 if(fn.crisp) return mf.compute_greq(mf,fn.crispvalue);
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = mf.compute_greq(mf,fn.conc[i].param[0]);
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = compute(fn,x);
   mu2 = mf.compute_eq(mf,x);
   if( mu2>greq ) greq = mu2;
   if( mu1<greq ) minmu = mu1; else minmu = greq;
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isSmallerOrEqual(MembershipFunction mf, FuzzyNumber fn) {
 int i;
 double x,mu1,mu2,minmu,degree=0,smeq=0;
 if(fn.crisp) return mf.compute_smeq(mf,fn.crispvalue);
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = mf.compute_smeq(mf,fn.conc[i].param[0]);
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.max; x>=mf.min; x-=mf.step){
   mu1 = compute(fn,x);
   mu2 = mf.compute_eq(mf,x);
   if( mu2>smeq ) smeq = mu2;
   if( mu1<smeq ) minmu = mu1; else minmu = smeq;
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isGreater(MembershipFunction mf, FuzzyNumber fn, OperatorSet op) {
 int i;
 double x,mu1,mu2,minmu,gr,degree=0,smeq=0;
 if(fn.crisp) return op.not(mf.compute_smeq(mf,fn.crispvalue));
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = op.not(mf.compute_smeq(mf,fn.conc[i].param[0]));
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.max; x>=mf.min; x-=mf.step){
   mu1 = compute(fn,x);
   mu2 = mf.compute_eq(mf,x);
   if( mu2>smeq ) smeq = mu2;
   gr = op.not(smeq);
   minmu = ( mu1<gr ? mu1 : gr);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isSmaller(MembershipFunction mf, FuzzyNumber fn, OperatorSet op) {
 int i;
 double x,mu1,mu2,minmu,sm,degree=0,greq=0;
 if(fn.crisp) return op.not(mf.compute_greq(mf,fn.crispvalue));
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = op.not(mf.compute_greq(mf,fn.conc[i].param[0]));
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = compute(fn,x);
   mu2 = mf.compute_eq(mf,x);
   if( mu2>greq ) greq = mu2;
   sm = op.not(greq);
   minmu = ( mu1<sm ? mu1 : sm);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isNotEqual(MembershipFunction mf, FuzzyNumber fn, OperatorSet op) {
 int i;
 double x,mu1,mu2,minmu,degree=0;
 if(fn.crisp) return op.not(mf.compute_eq(mf,fn.crispvalue));
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = op.not(mf.compute_eq(mf,fn.conc[i].param[0]));
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = compute(fn,x);
   mu2 = op.not(mf.compute_eq(mf,x));
   minmu = (mu1<mu2 ? mu1 : mu2);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isApproxEqual(MembershipFunction mf, FuzzyNumber fn, OperatorSet op) {
 int i;
 double x,mu1,mu2,minmu,degree=0;
 if(fn.crisp) return op.moreorless(mf.compute_eq(mf,fn.crispvalue));
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = op.moreorless(mf.compute_eq(mf,fn.conc[i].param[0]));
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = compute(fn,x);
   mu2 = op.moreorless(mf.compute_eq(mf,x));
   minmu = (mu1<mu2 ? mu1 : mu2);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isVeryEqual(MembershipFunction mf, FuzzyNumber fn, OperatorSet op) {
 int i;
 double x,mu1,mu2,minmu,degree=0;
 if(fn.crisp) return op.very(mf.compute_eq(mf,fn.crispvalue));
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = op.very(mf.compute_eq(mf,fn.conc[i].param[0]));
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = compute(fn,x);
   mu2 = op.very(mf.compute_eq(mf,x));
   minmu = (mu1<mu2 ? mu1 : mu2);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}

static double _isSlightlyEqual(MembershipFunction mf, FuzzyNumber fn, OperatorSet op) {
 int i;
 double x,mu1,mu2,minmu,degree=0;
 if(fn.crisp) return op.slightly(mf.compute_eq(mf,fn.crispvalue));
 if( isDiscreteFuzzyNumber(fn) ) {
  for(i=0; i<fn.length; i++){
   mu1 = op.slightly(mf.compute_eq(mf,fn.conc[i].param[0]));
   minmu = (mu1<fn.degree[i] ? mu1 : fn.degree[i]);
   if( degree<minmu ) degree = minmu;
  }
 }
 else {
  for(x=mf.min; x<=mf.max; x+=mf.step){
   mu1 = compute(fn,x);
   mu2 = op.slightly(mf.compute_eq(mf,x));
   minmu = (mu1<mu2 ? mu1 : mu2);
   if( degree<minmu ) degree = minmu;
  }
 }
 return degree;
}


/***************************************/
/*  MembershipFunction MF_xfl_triangle  */
/***************************************/
static double MF_xfl_triangle_equal(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
  return (a<x && x<=b? (x-a)/(b-a) : (b<x && x<c? (c-x)/(c-b) : 0)); 
}
static double MF_xfl_triangle_greq(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
  return (x<a? 0 : (x>b? 1 : (x-a)/(b-a) )); 
}
static double MF_xfl_triangle_smeq(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
  return (x<b? 1 : (x>c? 0 : (c-x)/(c-b) )); 
}
static double MF_xfl_triangle_center(MembershipFunction _mf) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
  return b; 
}
static double MF_xfl_triangle_basis(MembershipFunction _mf) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
  return (c-a); 
}
static MembershipFunction createMF_xfl_triangle( double min, double max, double step, double *param,int length) {
 int i;
 MembershipFunction _mf;
 _mf.min = min;
 _mf.max = max;
 _mf.step = step;
 _mf.param = (double*) malloc(length*sizeof(double));
 for(i=0;i<length;i++) _mf.param[i] = param[i];
 _mf.compute_eq = MF_xfl_triangle_equal;
 _mf.compute_greq = MF_xfl_triangle_greq;
 _mf.compute_smeq = MF_xfl_triangle_smeq;
 _mf.center = MF_xfl_triangle_center;
 _mf.basis = MF_xfl_triangle_basis;
 return _mf;
}

/***************************************/
/*  MembershipFunction MF_xfl_singleton  */
/***************************************/
static double MF_xfl_singleton_equal(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
  return (x==a? 1 : 0); 
}
static double MF_xfl_singleton_greq(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
  return (x>=a? 1 : 0); 
}
static double MF_xfl_singleton_smeq(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
  return (x<=a? 1 : 0); 
}
static double MF_xfl_singleton_center(MembershipFunction _mf) {
 double a = _mf.param[0];
  return a; 
}
static MembershipFunction createMF_xfl_singleton( double min, double max, double step, double *param,int length) {
 int i;
 MembershipFunction _mf;
 _mf.min = min;
 _mf.max = max;
 _mf.step = step;
 _mf.param = (double*) malloc(length*sizeof(double));
 for(i=0;i<length;i++) _mf.param[i] = param[i];
 _mf.compute_eq = MF_xfl_singleton_equal;
 _mf.compute_greq = MF_xfl_singleton_greq;
 _mf.compute_smeq = MF_xfl_singleton_smeq;
 _mf.center = MF_xfl_singleton_center;
 _mf.basis = _defaultMFbasis;
 return _mf;
}
/***************************************/
/*  Operatorset OP_opset */
/***************************************/

static double OP_opset_And(double a, double b) {
  return a*b; 
}

static double OP_opset_Or(double a, double b) {
  return (a>b? a : b); 
}

static double OP_opset_Also(double a, double b) {
  return (a>b? a : b); 
}

static double OP_opset_Imp(double a, double b) {
  return (a<b? a : b); 
}

static double OP_opset_Not(double a) {
  return 1-a; 
}

static double OP_opset_Very(double a) {
 return a*a; 
}

static double OP_opset_MoreOrLess(double a) {
 return a/2; 
}

static double OP_opset_Slightly(double a) {
  return 4*a*(1-a); 
}

static double OP_opset_Defuz(FuzzyNumber mf) {
 double min = mf.conc[0].min;
 double max = mf.conc[0].max;
   double num=0, denom=0;
   int i;
   for(i=0; i<mf.length; i++) {
    num += mf.degree[i] * center(mf.conc[i]);
    denom += mf.degree[i];
   }
   if(denom==0) return (min+max)/2;
   return num/denom;
}

static OperatorSet createOP_opset() {
 OperatorSet op;
 op.and = OP_opset_And;
 op.or = OP_opset_Or;
 op.also = OP_opset_Also;
 op.imp = OP_opset_Imp;
 op.not = OP_opset_Not;
 op.very = OP_opset_Very;
 op.moreorless = OP_opset_MoreOrLess;
 op.slightly = OP_opset_Slightly;
 op.defuz = OP_opset_Defuz;
 return op;
}


/***************************************/
/*  Type TP_Tx */
/***************************************/

static TP_Tx createTP_Tx() {
 TP_Tx tp;
 double min = -10.0;
 double max = 10.0;
 double step = 0.0784313725490196;
 double _p_mf0[3] = { -13.3,-10.0,-5.0 };
 double _p_mf1[3] = { -10.0,-5.0,-2.2 };
 double _p_mf2[3] = { -5.0,-2.2,0.0 };
 double _p_mf3[3] = { -2.2,0.0,2.2 };
 double _p_mf4[3] = { 0.0,2.2,5.0 };
 double _p_mf5[3] = { 2.2,5.0,10.0 };
 double _p_mf6[3] = { 5.0,10.0,13.3 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 tp.mf3 = createMF_xfl_triangle(min,max,step,_p_mf3,3);
 tp.mf4 = createMF_xfl_triangle(min,max,step,_p_mf4,3);
 tp.mf5 = createMF_xfl_triangle(min,max,step,_p_mf5,3);
 tp.mf6 = createMF_xfl_triangle(min,max,step,_p_mf6,3);
 return tp;
}

/***************************************/
/*  Type TP_Tangle */
/***************************************/

static TP_Tangle createTP_Tangle() {
 TP_Tangle tp;
 double min = -180.0;
 double max = 180.0;
 double step = 1.411764705882353;
 double _p_mf0[3] = { -240.0,-180.0,-130.0 };
 double _p_mf1[3] = { -180.0,-130.0,-50.0 };
 double _p_mf2[3] = { -130.0,-50.0,0.0 };
 double _p_mf3[3] = { -50.0,0.0,50.0 };
 double _p_mf4[3] = { 0.0,50.0,130.0 };
 double _p_mf5[3] = { 50.0,130.0,180.0 };
 double _p_mf6[3] = { 130.0,180.0,240.0 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 tp.mf3 = createMF_xfl_triangle(min,max,step,_p_mf3,3);
 tp.mf4 = createMF_xfl_triangle(min,max,step,_p_mf4,3);
 tp.mf5 = createMF_xfl_triangle(min,max,step,_p_mf5,3);
 tp.mf6 = createMF_xfl_triangle(min,max,step,_p_mf6,3);
 return tp;
}

/***************************************/
/*  Type TP_TY */
/***************************************/

static TP_TY createTP_TY() {
 TP_TY tp;
 double min = -2.0;
 double max = 30.0;
 double step = 0.12549019607843137;
 double _p_mfTG[3] = { -4.0,-2.0,0.0 };
 double _p_mfCL[3] = { -2.0,0.0,2.5 };
 double _p_mfNE[3] = { 0.0,2.5,5.0 };
 double _p_mfMI[3] = { 2.5,5.0,30.0 };
 double _p_mfFA[3] = { 5.0,30.0,35.0 };
 tp.mfTG = createMF_xfl_triangle(min,max,step,_p_mfTG,3);
 tp.mfCL = createMF_xfl_triangle(min,max,step,_p_mfCL,3);
 tp.mfNE = createMF_xfl_triangle(min,max,step,_p_mfNE,3);
 tp.mfMI = createMF_xfl_triangle(min,max,step,_p_mfMI,3);
 tp.mfFA = createMF_xfl_triangle(min,max,step,_p_mfFA,3);
 return tp;
}

/***************************************/
/*  Type TP_TGamma */
/***************************************/

static TP_TGamma createTP_TGamma() {
 TP_TGamma tp;
 double min = -0.4;
 double max = 0.4;
 double step = 0.0031372549019607846;
 double _p_mfNB[3] = { -0.5,-0.4,-0.2 };
 double _p_mfNS[3] = { -0.4,-0.2,-0.0 };
 double _p_mfZE[3] = { -0.2,0.0,0.2 };
 double _p_mfPS[3] = { 0.0,0.2,0.4 };
 double _p_mfPB[3] = { 0.2,0.4,0.5 };
 tp.mfNB = createMF_xfl_triangle(min,max,step,_p_mfNB,3);
 tp.mfNS = createMF_xfl_triangle(min,max,step,_p_mfNS,3);
 tp.mfZE = createMF_xfl_triangle(min,max,step,_p_mfZE,3);
 tp.mfPS = createMF_xfl_triangle(min,max,step,_p_mfPS,3);
 tp.mfPB = createMF_xfl_triangle(min,max,step,_p_mfPB,3);
 return tp;
}

/***************************************/
/*  Type TP_TWheel */
/***************************************/

static TP_TWheel createTP_TWheel() {
 TP_TWheel tp;
 double min = -0.4;
 double max = 0.4;
 double step = 0.0031372549019607846;
 double _p_mf0[1] = { -0.4 };
 double _p_mf1[1] = { 0.4 };
 double _p_mf2[1] = { 0.0 };
 tp.mf0 = createMF_xfl_singleton(min,max,step,_p_mf0,1);
 tp.mf1 = createMF_xfl_singleton(min,max,step,_p_mf1,1);
 tp.mf2 = createMF_xfl_singleton(min,max,step,_p_mf2,1);
 return tp;
}

/***************************************/
/*  Type TP_TV */
/***************************************/

static TP_TV createTP_TV() {
 TP_TV tp;
 double min = -1.0;
 double max = 0.0;
 double step = 0.00392156862745098;
 double _p_fast[1] = { -1.0 };
 double _p_quick[1] = { -0.75 };
 double _p_middle[1] = { -0.5 };
 double _p_slow[1] = { -0.25 };
 double _p_stop[1] = { 0.0 };
 tp.fast = createMF_xfl_singleton(min,max,step,_p_fast,1);
 tp.quick = createMF_xfl_singleton(min,max,step,_p_quick,1);
 tp.middle = createMF_xfl_singleton(min,max,step,_p_middle,1);
 tp.slow = createMF_xfl_singleton(min,max,step,_p_slow,1);
 tp.stop = createMF_xfl_singleton(min,max,step,_p_stop,1);
 return tp;
}

/***************************************/
/*  Rulebase RL_backward */
/***************************************/

static void RL_backward(FuzzyNumber x, FuzzyNumber angle, FuzzyNumber *wheel) {
 OperatorSet _op = createOP_opset();
 double _rl, _output;
 int _i_wheel=0;
 TP_Tx _t_x = createTP_Tx();
 TP_Tangle _t_angle = createTP_Tangle();
 TP_TWheel _t_wheel = createTP_TWheel();
 double *_input = (double*) malloc(2*sizeof(double));
 _input[0] = x.crispvalue;
 _input[1] = angle.crispvalue;
 *wheel = createFuzzyNumber(49,2,_input,_op);
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf2;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _output = _op.defuz(*wheel);
 free(wheel->degree);
 free(wheel->conc);
 *wheel = createCrispNumber(_output);
}

/***************************************/
/*  Rulebase RL_speed */
/***************************************/

static void RL_speed(FuzzyNumber y, FuzzyNumber gamma, FuzzyNumber *v) {
 OperatorSet _op = createOP_opset();
 double _rl, _output;
 int _i_v=0;
 TP_TY _t_y = createTP_TY();
 TP_TGamma _t_gamma = createTP_TGamma();
 TP_TV _t_v = createTP_TV();
 double *_input = (double*) malloc(2*sizeof(double));
 _input[0] = y.crispvalue;
 _input[1] = gamma.crispvalue;
 *v = createFuzzyNumber(25,2,_input,_op);
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfNB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.stop;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfNS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.stop;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfZE,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.stop;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfPS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.stop;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfTG,y),_isEqual(_t_gamma.mfPB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.stop;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfNB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfNS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfZE,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfPS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfCL,y),_isEqual(_t_gamma.mfPB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfNB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfNS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfZE,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfPS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfNE,y),_isEqual(_t_gamma.mfPB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.slow;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfNB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfNS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.quick;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfZE,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.fast;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfPS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.quick;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfMI,y),_isEqual(_t_gamma.mfPB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfNB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfNS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.quick;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfZE,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.fast;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfPS,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.quick;
 _i_v++;
 _rl = _op.and(_isEqual(_t_y.mfFA,y),_isEqual(_t_gamma.mfPB,gamma));
 v->degree[_i_v] = _rl;
 v->conc[_i_v] =  _t_v.middle;
 _i_v++;
 _output = _op.defuz(*v);
 free(v->degree);
 free(v->conc);
 *v = createCrispNumber(_output);
}

/***************************************/
/*          Inference Engine           */
/***************************************/

void ParkingBackwardInferenceEngine(double _d_x, double _d_angle, double _d_y, double _d_gamma, double _d_speed, double *_d_wheel, double *_d_v) {
 FuzzyNumber x = createCrispNumber(_d_x);
 FuzzyNumber angle = createCrispNumber(_d_angle);
 FuzzyNumber y = createCrispNumber(_d_y);
 FuzzyNumber gamma = createCrispNumber(_d_gamma);
 FuzzyNumber speed = createCrispNumber(_d_speed);
 FuzzyNumber wheel, v;
 RL_backward(x, angle, &wheel);
 RL_speed(y, gamma, &v);
 *_d_wheel = (wheel.crisp? wheel.crispvalue :wheel.op.defuz(wheel));
 *_d_v = (v.crisp? v.crispvalue :v.op.defuz(v));
}

